#include "video_recorder/video_recorder_node.hpp"
#include <filesystem>

namespace video_recorder {

VideoRecorderNode::VideoRecorderNode(const rclcpp::NodeOptions &options)
    : Node("video_recorder_node", options), writer_initialized_(false), frame_count_(0) {
  RCLCPP_INFO(this->get_logger(), "启动视频录制节点!");

  // 初始化参数
  last_time_ = this->now();
  
  // 声明参数
  this->declare_parameter("output_path", "./recorded_video.mp4");
  this->declare_parameter("video_format", "mp4v");
  this->declare_parameter("video_fps", 30);
  this->declare_parameter("video_width", 1440);  // 默认与相机分辨率一致
  this->declare_parameter("video_height", 1280); // 默认与相机分辨率一致
  
  // 获取参数
  output_path_ = this->get_parameter("output_path").as_string();
  video_format_ = this->get_parameter("video_format").as_string();
  video_fps_ = this->get_parameter("video_fps").as_int();
  video_width_ = this->get_parameter("video_width").as_int();
  video_height_ = this->get_parameter("video_height").as_int();
  
  // 确保输出目录存在
  std::filesystem::path output_file_path(output_path_);
  std::filesystem::create_directories(output_file_path.parent_path());
  
  RCLCPP_INFO(this->get_logger(), "视频将保存到: %s", output_path_.c_str());
  RCLCPP_INFO(this->get_logger(), "视频格式: %s, FPS: %d, 分辨率: %dx%d", 
              video_format_.c_str(), video_fps_, video_width_, video_height_);
  
  // 创建订阅者，订阅图像原始数据（/image_raw）
  image_sub_ = create_subscription<sensor_msgs::msg::Image>(
    "/darts/debug_image", rclcpp::SensorDataQoS(),
    std::bind(&VideoRecorderNode::imageCallback, this, std::placeholders::_1));
  
  RCLCPP_INFO(this->get_logger(), "已订阅 /darts/debug_image 话题");
}

VideoRecorderNode::~VideoRecorderNode() {
  // 释放视频写入器
  if (video_writer_.isOpened()) {
    RCLCPP_INFO(this->get_logger(), "关闭视频写入器，共录制 %zu 帧", frame_count_);
    video_writer_.release();
  }
}

void VideoRecorderNode::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
  // 帧率计算
  auto current_time = this->now();
  frame_count_++;
  double elapsed = (current_time - last_time_).seconds();
  
  if (elapsed >= 1.0) {
    double fps = frame_count_ / elapsed;
    RCLCPP_INFO(this->get_logger(), "[FPS] 当前: %.2f, 已录制帧数: %zu", fps, frame_count_);
    last_time_ = current_time;
  }

  try {
    // 使用cv_bridge将ROS图像消息转换为OpenCV图像
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    cv::Mat image = cv_ptr->image;
    
    // 如果视频写入器尚未初始化，则初始化它
    if (!writer_initialized_) {
      if (initVideoWriter(image.cols, image.rows)) {
        RCLCPP_INFO(this->get_logger(), "视频写入器初始化成功，开始录制...");
      } else {
        RCLCPP_ERROR(this->get_logger(), "视频写入器初始化失败!");
        return;
      }
    }
    
    // 写入视频帧
    video_writer_.write(image);
    
  } catch (cv_bridge::Exception &e) {
    RCLCPP_ERROR(this->get_logger(), "cv_bridge 异常: %s", e.what());
  }
}

bool VideoRecorderNode::initVideoWriter(int width, int height) {
  // 如果用户没有指定分辨率，使用图像的实际分辨率
  if (video_width_ <= 0 || video_height_ <= 0) {
    video_width_ = width;
    video_height_ = height;
  }
  
  // 获取当前时间并格式化为字符串，用于文件名
  auto now = std::chrono::system_clock::now();
  std::time_t now_time = std::chrono::system_clock::to_time_t(now);
  std::tm now_tm;
  #ifdef _WIN32
  localtime_s(&now_tm, &now_time);
  #else
  localtime_r(&now_time, &now_tm);
  #endif
  char time_str[100];
  std::strftime(time_str, sizeof(time_str), "%Y%m%d%H%M%S", &now_tm);
  
  // 如果输出路径没有包含时间戳，则添加时间戳
  std::filesystem::path output_file_path(output_path_);
  std::string filename = output_file_path.filename().string();
  std::string extension = output_file_path.extension().string();
  std::string stem = output_file_path.stem().string();
  
  // 创建带时间戳的文件名
  std::string timestamped_filename = stem + "_" + time_str + extension;
  std::filesystem::path parent_path = output_file_path.parent_path();
  std::filesystem::path timestamped_path = parent_path / timestamped_filename;
  
  // 获取视频编解码器的FourCC代码
  int fourcc = cv::VideoWriter::fourcc(video_format_[0], video_format_[1], 
                                      video_format_[2], video_format_[3]);
  
  // 初始化视频写入器
  video_writer_.open(timestamped_path.string(), fourcc, video_fps_, 
                    cv::Size(video_width_, video_height_));
  
  if (!video_writer_.isOpened()) {
    RCLCPP_ERROR(this->get_logger(), "无法创建视频文件: %s", timestamped_path.string().c_str());
    return false;
  }
  
  RCLCPP_INFO(this->get_logger(), "视频将保存到: %s", timestamped_path.string().c_str());
  writer_initialized_ = true;
  return true;
}

} // namespace video_recorder

#include "rclcpp_components/register_node_macro.hpp"

// 注册节点组件
RCLCPP_COMPONENTS_REGISTER_NODE(video_recorder::VideoRecorderNode)